#include "search.h"
#define FUZZY_SEG_NUMS 7   //7个模糊子集


typedef struct
{
    bool FUZZY_USED;   //是否使用模糊PID

    float max_e;    // e  \in [-max_e,max_e]
    float max_ec;   // ec \in [-max_ec,max_ec]
    float max_kp;   // kp \in kp0+[-max_kp,max_kp] kp0-max_kp>0
    float max_ki;
    float max_kd;

    uint8 rule_kp[FUZZY_SEG_NUMS][FUZZY_SEG_NUMS];  //模糊规则表
    uint8 rule_ki[FUZZY_SEG_NUMS][FUZZY_SEG_NUMS];
    uint8 rule_kd[FUZZY_SEG_NUMS][FUZZY_SEG_NUMS];

}fuzzy_pid_st;

typedef struct
{
    float e;
    float de;
    float out;

    float kp;   // kp基本值(对于模糊PID)
    float ki;   // ki基本值(对于模糊PID)
    float kd;   // kd基本值(对于模糊PID)

    float dt;
    float e_min;  // 积分限幅
    float e_max;
    float Integ_e;// 积分限幅
    float Integ_e_max;
    float damp;   //积分衰减系数 可设为0.9995之类的数

    fuzzy_pid_st fuzzy_t;  // 模糊PID
} PID_pm_st;


/*模糊化*/
void fuzzification(float val, float max, float* percent)  //模糊化
{
    float interval[FUZZY_SEG_NUMS];

    float seg_len = 2 * max / (FUZZY_SEG_NUMS - 1);
    for (int i = 0; i < FUZZY_SEG_NUMS; i++)
    {
        percent[i] = 0;                    // percent赋初值
        interval[i] = -max + i * seg_len;  //分段点的值 NB NM NS ZO PS PM PB 的值
    }
    if (val <= interval[0])  // <= 0
    {
        percent[0] = 1;
        return;
    }
    else if (val <= interval[1])  //  [0,1]
    {
        percent[1] = (val - interval[0]) / seg_len;
        percent[0] = 1 - percent[1];
        return;
    }
    else if (val <= interval[2])  // [1,2]
    {
        percent[2] = (val - interval[1]) / seg_len;
        percent[1] = 1 - percent[2];
        return;
    }
    else if (val <= interval[3])  // [2,3]
    {
        percent[3] = (val - interval[2]) / seg_len;
        percent[2] = 1 - percent[3];
        return;
    }
    else if (val <= interval[4])  // [3,4]
    {
        percent[4] = (val - interval[3]) / seg_len;
        percent[3] = 1 - percent[4];
        return;
    }
    else if (val <= interval[5])  // [4,5]
    {
        percent[5] = (val - interval[4]) / seg_len;
        percent[4] = 1 - percent[5];
        return;
    }
    else if (val <= interval[6])  // [5,6]
    {
        percent[6] = (val - interval[5]) / seg_len;
        percent[5] = 1 - percent[6];
        return;
    }
    else                          // >6
    {
        percent[6] = 1;
        return;
    }
}

/*根据模糊规则表，将e和ec映射为响应的控制参数：kp ki kd*/
void fuzzy_rule(const float* percent_e, const float* percent_ec, const uint8 rule[FUZZY_SEG_NUMS][FUZZY_SEG_NUMS], float* percent_out)  //模糊规则表映射
{
    for (int i = 0; i < FUZZY_SEG_NUMS; i++)
    {
        percent_out[i] = 0;                    // percent赋初值
    }
    for (int i = 0; i < FUZZY_SEG_NUMS; i++)   // e
    {
        for (int j = 0; j < FUZZY_SEG_NUMS; j++)  //ec
        {
            if (percent_e[i] == 0 || percent_ec[j] == 0)
                continue;
            uint8 index = rule[i][j];
            percent_out[index] += percent_e[i] * percent_ec[j];
        }
    }
}

/*去模糊化，模糊化相反的过程*/
float de_fuzzification(float max, float* percent)  //去模糊化
{
    float val = 0;
    float interval[FUZZY_SEG_NUMS];
    float seg_len = 2 * max / (FUZZY_SEG_NUMS - 1);
    for (int i = 0; i < FUZZY_SEG_NUMS; i++)
    {
        interval[i] = -max + i * seg_len;  //分段点的值 NB NM NS ZO PS PM PB 的值

        val += percent[i] * interval[i];
    }
    return val;
}

/* 浮点数绝对值 */
float abs_1f(float val)
{
    return (val > 0 ? val : -val);
}
/* 浮点数符号 */
float sign_1f(float val)
{
    return (val > 0 ? 1.0f : -1.0f);
}
/* 浮点数限幅 */
float limit_abs1f(float val, float limit)
{
    if (abs_1f(val) > limit)
        return (sign_1f(val) * limit);
    else
        return val;
}

/* PID控制器 */
void PID_control(PID_pm_st* PID_pm)
{
    float kp, ki, kd;
    kp = PID_pm->kp;
    ki = PID_pm->ki;
    kd = PID_pm->kd;
    /* 根据模糊规则表计算kp ki kd */
    if (PID_pm->fuzzy_t.FUZZY_USED == true)
    {
        float percent_e[7];
        float percent_ec[7];
        float percent_kp[7];
        float percent_ki[7];
        float percent_kd[7];

        float d_kp, d_ki, d_kd;

        /* e和ec模糊化 */
        fuzzification(PID_pm->e, PID_pm->fuzzy_t.max_e, percent_e);
        fuzzification(PID_pm->de, PID_pm->fuzzy_t.max_ec, percent_ec);

        /* 根据模糊规则计算参数 */
        fuzzy_rule(percent_e, percent_ec, PID_pm->fuzzy_t.rule_kp, percent_kp);
        fuzzy_rule(percent_e, percent_ec, PID_pm->fuzzy_t.rule_ki, percent_ki);
        fuzzy_rule(percent_e, percent_ec, PID_pm->fuzzy_t.rule_kd, percent_kd);

        /* kp ki kd 去模糊化*/
        d_kp = de_fuzzification(PID_pm->fuzzy_t.max_kp, percent_kp);
        d_ki = de_fuzzification(PID_pm->fuzzy_t.max_ki, percent_ki);
        d_kd = de_fuzzification(PID_pm->fuzzy_t.max_kd, percent_kd);
        kp += d_kp;
        ki += d_ki;
        kd += d_kd;

    }

    PID_pm->Integ_e += limit_abs1f(PID_pm->e, PID_pm->e_max) * PID_pm->dt; // 【积分分离】限制e的幅值
    PID_pm->Integ_e = limit_abs1f(PID_pm->Integ_e, PID_pm->Integ_e_max);   // 限制integ_e的幅值
    PID_pm->Integ_e = PID_pm->Integ_e * PID_pm->damp;                      //damp为积分随着时间的衰减系数
    PID_pm->out = kp * PID_pm->e + kd * PID_pm->de + ki * PID_pm->Integ_e;
}